import numpy as np
import matplotlib.pyplot as plt

# 参考http://zhuanlan.zhihu.com/p/506240030
def svpwm(alpha, beta):
    """
    SVPWM算法实现
    
    参数:
    alpha: α轴电压分量(标幺值，取值范围[-1, 1])
    beta: β轴电压分量(标幺值，取值范围[-1, 1])
    
    当alpha^2 + beta^2 > ( sqrt(3)/2 )^2 时，输出波形会失真

    返回:
    U, V, W: 三相电压占空比(取值范围[0, 1])
    """
    # 常量定义
    sqrt3 = np.sqrt(3)
    one_by_sqrt3 = 1.0 / sqrt3
    two_by_sqrt3 = 2.0 / sqrt3

    # 确定扇区
    if beta >= 0.0:
        if alpha >= 0.0:
            if one_by_sqrt3 * beta > alpha:
                sector = 2
            else:
                sector = 1
        else:
            if -one_by_sqrt3 * beta > alpha:
                sector = 3
            else:
                sector = 2
    else:
        if alpha >= 0.0:
            if -one_by_sqrt3 * beta > alpha:
                sector = 5 
            else:
                sector = 6
        else:
            if one_by_sqrt3 * beta > alpha:
                sector = 4
            else:
                sector = 5
    
    # 根据扇区计算占空比
    if sector == 1:
        t1 = alpha - one_by_sqrt3 * beta
        t2 = two_by_sqrt3 * beta
        tA = (1.0 - t1 - t2) * 0.5
        tB = tA + t1
        tC = tB + t2
        
    elif sector == 2:
        t2 = alpha + one_by_sqrt3 * beta
        t3 = -alpha + one_by_sqrt3 * beta
        tB = (1.0 - t2 - t3) * 0.5
        tA = tB + t3
        tC = tA + t2
        
    elif sector == 3:
        t3 = two_by_sqrt3 * beta
        t4 = -alpha - one_by_sqrt3 * beta
        tB = (1.0 - t3 - t4) * 0.5
        tC = tB + t3
        tA = tC + t4
        
    elif sector == 4:
        t4 = -alpha + one_by_sqrt3 * beta
        t5 = -two_by_sqrt3 * beta
        tC = (1.0 - t4 - t5) * 0.5
        tB = tC + t5
        tA = tB + t4
        
    elif sector == 5:
        t5 = -alpha - one_by_sqrt3 * beta
        t6 = alpha - one_by_sqrt3 * beta
        tC = (1.0 - t5 - t6) * 0.5
        tA = tC + t5
        tB = tA + t6
        
    else: 
        t6 = -two_by_sqrt3 * beta
        t1 = alpha + one_by_sqrt3 * beta
        tA = (1.0 - t6 - t1) * 0.5
        tC = tA + t1
        tB = tC + t6
    
    # 确保占空比在[0,1]范围内
    tA = max(0.0, min(1.0, tA))
    tB = max(0.0, min(1.0, tB))
    tC = max(0.0, min(1.0, tC))
    
    return tA, tB, tC

# 逆Park变换：将Ud/Uq转换为alpha/beta
# Vα ​= Vd​cos(θ)  − Vq​sin(θ)  
# Vβ ​= Vd​sin(θ)  + Vq​cos(θ)
def inv_park(Ud_input, Uq_input, theta, Udc):
    # 标幺化处理（基准值 = 2/3*Udc, 最大相电压）
    # base表示在 SVPWM 控制下，逆变器能输出的​​最大相电压峰值​​
    # 当采用 SVPWM 调制时，最大不失真相电压幅值为直流母线电压Udc的1/sqrt(3)
    # 但算法中通过 2/3*Udc进行归一化，是因为该值对应​基本空间矢量的幅值​​（如矢量 100, 110 等的幅值）
    base = 2/3 * Udc
    Ud = Ud_input / base
    Uq = Uq_input / base
    alpha = Ud * np.cos(theta) - Uq * np.sin(theta)
    beta = Ud * np.sin(theta) + Uq * np.cos(theta)
    return alpha, beta

# Clark变换 a,b,c -> α,β
def clark(a, b, c):
    one_by_sqrt3 = 1.0 / np.sqrt(3)
    alpha = a
    beta = one_by_sqrt3 * (b - c)
    return alpha, beta
# 已知a+b+c=0, TODO 编写三个版本：
# 输入参数只有a,b
# 输入参数只有a,c
# 输入参数只有b,c

# Park变换：alpha/beta -> Ud/Uq
def park(alpha, beta, theta):
    Ud = alpha * np.cos(theta) + beta * np.sin(theta)
    Uq = -alpha * np.sin(theta) + beta * np.cos(theta)
    return Ud, Uq

def main():
    """
    主函数 - SVPWM验证程序
    """
    print("SVPWM验证程序启动...")
    
    # 生成测试信号
    print("生成测试信号...")
    frequency = 1
    num_points = 1000
    time = np.linspace(0, 4*np.pi, num_points)
    theta = frequency * time
    
    # 计算SVPWM输出
    print("计算SVPWM输出...")
    alpha = np.zeros_like(theta)
    beta = np.zeros_like(theta)
    U = np.zeros_like(theta)
    V = np.zeros_like(theta)
    W = np.zeros_like(theta)
    Udc = 24
    Uq = 24/np.sqrt(3) # max: Udc / sqrt(3)
    Ud = 0
    
    # 用于验证的数组
    alpha_recon = np.zeros_like(theta)
    beta_recon = np.zeros_like(theta)
    Ua_phase = np.zeros_like(theta)
    Ub_phase = np.zeros_like(theta)
    Uc_phase = np.zeros_like(theta)
    Ud_recon = np.zeros_like(theta)
    Uq_recon = np.zeros_like(theta)

    for i in range(len(theta)):
        alpha[i], beta[i] = inv_park(Ud, Uq, theta[i], Udc)
        # U[i], V[i], W[i]表示下MOS的导通占空比
        U[i], V[i], W[i] = svpwm(alpha[i], beta[i])
        
        # 从三相占空比重构αβ分量
        # 计算三相电压 (相对于直流母线负端)
        Ua = -Udc * U[i]
        Ub = -Udc * V[i]
        Uc = -Udc * W[i]
        
        # 计算中点电压
        U0 = (Ua + Ub + Uc) / 3
        
        # 计算相电压 (相对于电机中点)
        Ua_phase[i] = Ua - U0
        Ub_phase[i] = Ub - U0
        Uc_phase[i] = Uc - U0
        
        # 通过Clark变换重构αβ分量
        alpha_recon[i], beta_recon[i] = clark(Ua_phase[i], Ub_phase[i], Uc_phase[i])
        # 通过Park变换变换重构Uq、Ud分量
        Ud_recon[i], Uq_recon[i] = park(alpha_recon[i], beta_recon[i], theta[i])
    
    # 计算重构误差
    # TODO：验证Uq、Ud误差
    alpha_error = np.abs(alpha * (2/3 * Udc) - alpha_recon)  # 将标幺值转换为实际电压值进行比较
    beta_error = np.abs(beta * (2/3 * Udc) - beta_recon)      # 将标幺值转换为实际电压值进行比较
    Ud_error = np.abs(Ud - Ud_recon)
    Uq_error = np.abs(Uq - Uq_recon)
    max_alpha_error = np.max(alpha_error)
    max_beta_error = np.max(beta_error)
    max_Ud_error = np.max(Ud_error)
    max_Uq_error = np.max(Uq_error)
    print("\n===== 验证结果 =====")
    print(f"最大α重构误差: {max_alpha_error:.6f} V")
    print(f"最大β重构误差: {max_beta_error:.6f} V")
    print(f"最大Ud重构误差: {max_Ud_error:.6f} V")
    print(f"最大Uq重构误差: {max_Uq_error:.6f} V")

    # 验证相电压峰值
    expected_peak = np.sqrt(Ud**2 + Uq**2)
    a_peak = (np.max(Ua_phase) - np.min(Ua_phase)) / 2
    b_peak = (np.max(Ub_phase) - np.min(Ub_phase)) / 2
    c_peak = (np.max(Uc_phase) - np.min(Uc_phase)) / 2
    print(f"\n期望相电压峰值: {expected_peak:.3f} V")
    print(f"U、V、W实际相电压峰值: {a_peak:.3f} V， {b_peak:.3f} V，{c_peak:.3f} V")
    
    
    
    # 创建图形
    fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(12, 9))
    
    # 绘制αβ轴电压
    ax1.plot(time, alpha * (2/3 * Udc), 'b-', linewidth=2, label='Original Alpha')
    ax1.plot(time, beta * (2/3 * Udc), 'r-', linewidth=2, label='Original Beta')
    ax1.plot(time, alpha_recon, 'c--', linewidth=1.5, label='Reconstructed Alpha')
    ax1.plot(time, beta_recon, 'm--', linewidth=1.5, label='Reconstructed Beta')
    ax1.set_title('SVPWM - Alpha Beta Voltages', fontsize=14)
    ax1.set_xlabel('Time (rad)')
    ax1.set_ylabel('Voltage')
    ax1.legend()
    ax1.grid(True, alpha=0.3)
    
    # 绘制三相电压（PWM）
    ax2.plot(time, U, 'r-', linewidth=2, label='U Phase')
    ax2.plot(time, V, 'g-', linewidth=2, label='V Phase')
    ax2.plot(time, W, 'b-', linewidth=2, label='W Phase')
    ax2.set_title('SVPWM', fontsize=14)
    ax2.set_xlabel('Time (rad)')
    ax2.set_ylabel('Voltage (Duty Cycle)')
    ax2.legend()
    ax2.grid(True, alpha=0.3)

    # 绘制三相电压（相电压）
    ax3.plot(time, Ua_phase, 'r-', linewidth=2, label='U Phase')
    ax3.plot(time, Ub_phase, 'g-', linewidth=2, label='V Phase')
    ax3.plot(time, Uc_phase, 'b-', linewidth=2, label='W Phase')
    ax3.set_title('SVPWM - Phase Voltages', fontsize=14)
    ax3.set_xlabel('Time (rad)')
    ax3.set_ylabel('Voltage (V)')
    ax3.legend()
    ax3.grid(True, alpha=0.3)
    plt.tight_layout()
    plt.show()
    
    # 绘制电压矢量轨迹
    # plt.figure(figsize=(8, 8))
    # plt.plot(alpha, beta, 'b-', linewidth=1, alpha=0.7, label='Original αβ')
    # if alpha_recon is not None and beta_recon is not None:
    #     plt.plot(alpha_recon, beta_recon, 'r--', linewidth=1, alpha=0.7, label='Reconstructed αβ')
    # plt.title('Voltage Vector Trajectory', fontsize=14)
    # plt.xlabel('Alpha (α)')
    # plt.ylabel('Beta (β)')
    # plt.axis('equal')
    # plt.grid(True, alpha=0.3)
    # plt.legend()
    # plt.show()

    # alpha, beta = inv_park(24/np.sqrt(3), 0, 0, 24)
    # print(svpwm(alpha, beta))
    # (np.float64(0.06698729810778065), np.float64(0.9330127018922194), np.float64(0.9330127018922194))

if __name__ == "__main__":
    main()